function out = saveStatesControls(thetaVec,setup)
 
%% Getting some information from setup 
scaleX_in_g = setup.scaleX_in_g;
x_cu        = setup.xGrid;
numX        = size(x_cu,1);
nNodes      = setup.n_nodes;
epsNodes    = setup.epsi_nodes;
params      = setup.params;
appOrder    = setup.appOrder;
nx          = setup.nx;
ny          = setup.ny;
nx1         = setup.nx1;
gSS         = setup.gSS;
hSS         = setup.hSS;
% Unfolding thetaVec
thetaProj   = zeros(setup.nDim1Theta,setup.nDim2Theta);
countIdx = 0;
for i=1:setup.nDim1Theta
    nn = countIdx + sum(setup.select(i,:));
    thetaProj(i,setup.select(i,:) == 1) = thetaVec(countIdx+1:nn);
    countIdx = nn;
end
g0          = thetaProj(:,1); 
gx          = thetaProj(:,2:1+nx); 
if appOrder > 1 
    gxxV    = thetaProj(:,nx+2:1+nx+nx*(nx+1)/2); 
end 
if appOrder > 2 
    gxxxV   = thetaProj(:,1+nx+nx*(nx+1)/2+1:1+nx+nx*(nx+1)/2+nx*(nx+1)*(nx+2)/6); 
end 
 
%Unfold params 
BETTA= params.BETTA;
B= params.B;
CHI= params.CHI;
CHI0= params.CHI0;
THETA= params.THETA;
DELTA= params.DELTA;
ALFA= params.ALFA;
PHI= params.PHI;
PHIzero= params.PHIzero;
ZI= params.ZI;
KAPAw= params.KAPAw;
ETA= params.ETA;
RHOR= params.RHOR;
PHIpai= params.PHIpai;
PHIpai_1= params.PHIpai_1;
PHIy= params.PHIy;
PHIy_1= params.PHIy_1;
PHIc= params.PHIc;
PHIc_1= params.PHIc_1;
PHIl= params.PHIl;
NU= params.NU;
U0= params.U0;
U0d= params.U0d;
OMEGAz= params.OMEGAz;
OMEGAd= params.OMEGAd;
OMEGAn= params.OMEGAn;
OMEGAp= params.OMEGAp;
OMEGAa= params.OMEGAa;
RHOz= params.RHOz;
RHOd= params.RHOd;
RHOn= params.RHOn;
RHOp= params.RHOp;
RHOa= params.RHOa;
Kss= params.Kss;
OUTPUTss= params.OUTPUTss;
PAIss= params.PAIss;
MUZss= params.MUZss;
Css= params.Css;
AA= params.AA;
lss= params.lss;
Rss= params.Rss;
Wss= params.Wss;
 
%% Computing the various variables 
 
% Getting the states at time t 
c_ba1 = hSS(1,1) + repmat(x_cu(:,1),1,nNodes);
muz_cu = hSS(2,1) + repmat(x_cu(:,2),1,nNodes);
d_cu = hSS(3,1) + repmat(x_cu(:,3),1,nNodes);
n_cu = hSS(4,1) + repmat(x_cu(:,4),1,nNodes);
paistar_cu = hSS(5,1) + repmat(x_cu(:,5),1,nNodes);
a_cu = hSS(6,1) + repmat(x_cu(:,6),1,nNodes);
 
% Getting the controls at time t and part of the states at time t+1  
% yVec_cu: numX * ny 
x1_cu  = x_cu(:,1)/scaleX_in_g(1,1);
x2_cu  = x_cu(:,2)/scaleX_in_g(2,1);
x3_cu  = x_cu(:,3)/scaleX_in_g(3,1);
x4_cu  = x_cu(:,4)/scaleX_in_g(4,1);
x5_cu  = x_cu(:,5)/scaleX_in_g(5,1);
x6_cu  = x_cu(:,6)/scaleX_in_g(6,1);
 
c_cu = g0(1,1)+gx(1,1).*x1_cu+gx(1,2).*x2_cu+gx(1,3).*x3_cu+gx(1,4).*x4_cu+gx(1,5).*x5_cu+gx(1,6).*x6_cu;
pai_cu = g0(2,1)+gx(2,1).*x1_cu+gx(2,2).*x2_cu+gx(2,3).*x3_cu+gx(2,4).*x4_cu+gx(2,5).*x5_cu+gx(2,6).*x6_cu;
evf_cu = g0(3,1)+gx(3,1).*x1_cu+gx(3,2).*x2_cu+gx(3,3).*x3_cu+gx(3,4).*x4_cu+gx(3,5).*x5_cu+gx(3,6).*x6_cu;
if appOrder > 1
    c_cu = c_cu+gxxV(1,1).*x1_cu.*x1_cu+2.*gxxV(1,2).*x1_cu.*x2_cu+2.*gxxV(1,3).*x1_cu.*x3_cu+2.*gxxV(1,4).*x1_cu.*x4_cu+2.*gxxV(1,5).*x1_cu.*x5_cu+2.*gxxV(1,6).*x1_cu.*x6_cu+gxxV(1,7).*x2_cu.*x2_cu+2.*gxxV(1,8).*x2_cu.*x3_cu+2.*gxxV(1,9).*x2_cu.*x4_cu+2.*gxxV(1,10).*x2_cu.*x5_cu+2.*gxxV(1,11).*x2_cu.*x6_cu+gxxV(1,12).*x3_cu.*x3_cu+2.*gxxV(1,13).*x3_cu.*x4_cu+2.*gxxV(1,14).*x3_cu.*x5_cu+2.*gxxV(1,15).*x3_cu.*x6_cu+gxxV(1,16).*x4_cu.*x4_cu+2.*gxxV(1,17).*x4_cu.*x5_cu+2.*gxxV(1,18).*x4_cu.*x6_cu+gxxV(1,19).*x5_cu.*x5_cu+2.*gxxV(1,20).*x5_cu.*x6_cu+gxxV(1,21).*x6_cu.*x6_cu;
    pai_cu = pai_cu+gxxV(2,1).*x1_cu.*x1_cu+2.*gxxV(2,2).*x1_cu.*x2_cu+2.*gxxV(2,3).*x1_cu.*x3_cu+2.*gxxV(2,4).*x1_cu.*x4_cu+2.*gxxV(2,5).*x1_cu.*x5_cu+2.*gxxV(2,6).*x1_cu.*x6_cu+gxxV(2,7).*x2_cu.*x2_cu+2.*gxxV(2,8).*x2_cu.*x3_cu+2.*gxxV(2,9).*x2_cu.*x4_cu+2.*gxxV(2,10).*x2_cu.*x5_cu+2.*gxxV(2,11).*x2_cu.*x6_cu+gxxV(2,12).*x3_cu.*x3_cu+2.*gxxV(2,13).*x3_cu.*x4_cu+2.*gxxV(2,14).*x3_cu.*x5_cu+2.*gxxV(2,15).*x3_cu.*x6_cu+gxxV(2,16).*x4_cu.*x4_cu+2.*gxxV(2,17).*x4_cu.*x5_cu+2.*gxxV(2,18).*x4_cu.*x6_cu+gxxV(2,19).*x5_cu.*x5_cu+2.*gxxV(2,20).*x5_cu.*x6_cu+gxxV(2,21).*x6_cu.*x6_cu;
    evf_cu = evf_cu+gxxV(3,1).*x1_cu.*x1_cu+2.*gxxV(3,2).*x1_cu.*x2_cu+2.*gxxV(3,3).*x1_cu.*x3_cu+2.*gxxV(3,4).*x1_cu.*x4_cu+2.*gxxV(3,5).*x1_cu.*x5_cu+2.*gxxV(3,6).*x1_cu.*x6_cu+gxxV(3,7).*x2_cu.*x2_cu+2.*gxxV(3,8).*x2_cu.*x3_cu+2.*gxxV(3,9).*x2_cu.*x4_cu+2.*gxxV(3,10).*x2_cu.*x5_cu+2.*gxxV(3,11).*x2_cu.*x6_cu+gxxV(3,12).*x3_cu.*x3_cu+2.*gxxV(3,13).*x3_cu.*x4_cu+2.*gxxV(3,14).*x3_cu.*x5_cu+2.*gxxV(3,15).*x3_cu.*x6_cu+gxxV(3,16).*x4_cu.*x4_cu+2.*gxxV(3,17).*x4_cu.*x5_cu+2.*gxxV(3,18).*x4_cu.*x6_cu+gxxV(3,19).*x5_cu.*x5_cu+2.*gxxV(3,20).*x5_cu.*x6_cu+gxxV(3,21).*x6_cu.*x6_cu;
end
if appOrder > 2
    c_cu = c_cu+gxxxV(1,1).*x1_cu.*x1_cu.*x1_cu+3.*gxxxV(1,2).*x1_cu.*x1_cu.*x2_cu+3.*gxxxV(1,3).*x1_cu.*x1_cu.*x3_cu+3.*gxxxV(1,4).*x1_cu.*x1_cu.*x4_cu+3.*gxxxV(1,5).*x1_cu.*x1_cu.*x5_cu+3.*gxxxV(1,6).*x1_cu.*x1_cu.*x6_cu+3.*gxxxV(1,7).*x1_cu.*x2_cu.*x2_cu+6.*gxxxV(1,8).*x1_cu.*x2_cu.*x3_cu+6.*gxxxV(1,9).*x1_cu.*x2_cu.*x4_cu+6.*gxxxV(1,10).*x1_cu.*x2_cu.*x5_cu+6.*gxxxV(1,11).*x1_cu.*x2_cu.*x6_cu+3.*gxxxV(1,12).*x1_cu.*x3_cu.*x3_cu+6.*gxxxV(1,13).*x1_cu.*x3_cu.*x4_cu+6.*gxxxV(1,14).*x1_cu.*x3_cu.*x5_cu+6.*gxxxV(1,15).*x1_cu.*x3_cu.*x6_cu+3.*gxxxV(1,16).*x1_cu.*x4_cu.*x4_cu+6.*gxxxV(1,17).*x1_cu.*x4_cu.*x5_cu+6.*gxxxV(1,18).*x1_cu.*x4_cu.*x6_cu+3.*gxxxV(1,19).*x1_cu.*x5_cu.*x5_cu+6.*gxxxV(1,20).*x1_cu.*x5_cu.*x6_cu+3.*gxxxV(1,21).*x1_cu.*x6_cu.*x6_cu+gxxxV(1,22).*x2_cu.*x2_cu.*x2_cu+3.*gxxxV(1,23).*x2_cu.*x2_cu.*x3_cu+3.*gxxxV(1,24).*x2_cu.*x2_cu.*x4_cu+3.*gxxxV(1,25).*x2_cu.*x2_cu.*x5_cu+3.*gxxxV(1,26).*x2_cu.*x2_cu.*x6_cu+3.*gxxxV(1,27).*x2_cu.*x3_cu.*x3_cu+6.*gxxxV(1,28).*x2_cu.*x3_cu.*x4_cu+6.*gxxxV(1,29).*x2_cu.*x3_cu.*x5_cu+6.*gxxxV(1,30).*x2_cu.*x3_cu.*x6_cu+3.*gxxxV(1,31).*x2_cu.*x4_cu.*x4_cu+6.*gxxxV(1,32).*x2_cu.*x4_cu.*x5_cu+6.*gxxxV(1,33).*x2_cu.*x4_cu.*x6_cu+3.*gxxxV(1,34).*x2_cu.*x5_cu.*x5_cu+6.*gxxxV(1,35).*x2_cu.*x5_cu.*x6_cu+3.*gxxxV(1,36).*x2_cu.*x6_cu.*x6_cu+gxxxV(1,37).*x3_cu.*x3_cu.*x3_cu+3.*gxxxV(1,38).*x3_cu.*x3_cu.*x4_cu+3.*gxxxV(1,39).*x3_cu.*x3_cu.*x5_cu+3.*gxxxV(1,40).*x3_cu.*x3_cu.*x6_cu+3.*gxxxV(1,41).*x3_cu.*x4_cu.*x4_cu+6.*gxxxV(1,42).*x3_cu.*x4_cu.*x5_cu+6.*gxxxV(1,43).*x3_cu.*x4_cu.*x6_cu+3.*gxxxV(1,44).*x3_cu.*x5_cu.*x5_cu+6.*gxxxV(1,45).*x3_cu.*x5_cu.*x6_cu+3.*gxxxV(1,46).*x3_cu.*x6_cu.*x6_cu+gxxxV(1,47).*x4_cu.*x4_cu.*x4_cu+3.*gxxxV(1,48).*x4_cu.*x4_cu.*x5_cu+3.*gxxxV(1,49).*x4_cu.*x4_cu.*x6_cu+3.*gxxxV(1,50).*x4_cu.*x5_cu.*x5_cu+6.*gxxxV(1,51).*x4_cu.*x5_cu.*x6_cu+3.*gxxxV(1,52).*x4_cu.*x6_cu.*x6_cu+gxxxV(1,53).*x5_cu.*x5_cu.*x5_cu+3.*gxxxV(1,54).*x5_cu.*x5_cu.*x6_cu+3.*gxxxV(1,55).*x5_cu.*x6_cu.*x6_cu+gxxxV(1,56).*x6_cu.*x6_cu.*x6_cu;
    pai_cu = pai_cu+gxxxV(2,1).*x1_cu.*x1_cu.*x1_cu+3.*gxxxV(2,2).*x1_cu.*x1_cu.*x2_cu+3.*gxxxV(2,3).*x1_cu.*x1_cu.*x3_cu+3.*gxxxV(2,4).*x1_cu.*x1_cu.*x4_cu+3.*gxxxV(2,5).*x1_cu.*x1_cu.*x5_cu+3.*gxxxV(2,6).*x1_cu.*x1_cu.*x6_cu+3.*gxxxV(2,7).*x1_cu.*x2_cu.*x2_cu+6.*gxxxV(2,8).*x1_cu.*x2_cu.*x3_cu+6.*gxxxV(2,9).*x1_cu.*x2_cu.*x4_cu+6.*gxxxV(2,10).*x1_cu.*x2_cu.*x5_cu+6.*gxxxV(2,11).*x1_cu.*x2_cu.*x6_cu+3.*gxxxV(2,12).*x1_cu.*x3_cu.*x3_cu+6.*gxxxV(2,13).*x1_cu.*x3_cu.*x4_cu+6.*gxxxV(2,14).*x1_cu.*x3_cu.*x5_cu+6.*gxxxV(2,15).*x1_cu.*x3_cu.*x6_cu+3.*gxxxV(2,16).*x1_cu.*x4_cu.*x4_cu+6.*gxxxV(2,17).*x1_cu.*x4_cu.*x5_cu+6.*gxxxV(2,18).*x1_cu.*x4_cu.*x6_cu+3.*gxxxV(2,19).*x1_cu.*x5_cu.*x5_cu+6.*gxxxV(2,20).*x1_cu.*x5_cu.*x6_cu+3.*gxxxV(2,21).*x1_cu.*x6_cu.*x6_cu+gxxxV(2,22).*x2_cu.*x2_cu.*x2_cu+3.*gxxxV(2,23).*x2_cu.*x2_cu.*x3_cu+3.*gxxxV(2,24).*x2_cu.*x2_cu.*x4_cu+3.*gxxxV(2,25).*x2_cu.*x2_cu.*x5_cu+3.*gxxxV(2,26).*x2_cu.*x2_cu.*x6_cu+3.*gxxxV(2,27).*x2_cu.*x3_cu.*x3_cu+6.*gxxxV(2,28).*x2_cu.*x3_cu.*x4_cu+6.*gxxxV(2,29).*x2_cu.*x3_cu.*x5_cu+6.*gxxxV(2,30).*x2_cu.*x3_cu.*x6_cu+3.*gxxxV(2,31).*x2_cu.*x4_cu.*x4_cu+6.*gxxxV(2,32).*x2_cu.*x4_cu.*x5_cu+6.*gxxxV(2,33).*x2_cu.*x4_cu.*x6_cu+3.*gxxxV(2,34).*x2_cu.*x5_cu.*x5_cu+6.*gxxxV(2,35).*x2_cu.*x5_cu.*x6_cu+3.*gxxxV(2,36).*x2_cu.*x6_cu.*x6_cu+gxxxV(2,37).*x3_cu.*x3_cu.*x3_cu+3.*gxxxV(2,38).*x3_cu.*x3_cu.*x4_cu+3.*gxxxV(2,39).*x3_cu.*x3_cu.*x5_cu+3.*gxxxV(2,40).*x3_cu.*x3_cu.*x6_cu+3.*gxxxV(2,41).*x3_cu.*x4_cu.*x4_cu+6.*gxxxV(2,42).*x3_cu.*x4_cu.*x5_cu+6.*gxxxV(2,43).*x3_cu.*x4_cu.*x6_cu+3.*gxxxV(2,44).*x3_cu.*x5_cu.*x5_cu+6.*gxxxV(2,45).*x3_cu.*x5_cu.*x6_cu+3.*gxxxV(2,46).*x3_cu.*x6_cu.*x6_cu+gxxxV(2,47).*x4_cu.*x4_cu.*x4_cu+3.*gxxxV(2,48).*x4_cu.*x4_cu.*x5_cu+3.*gxxxV(2,49).*x4_cu.*x4_cu.*x6_cu+3.*gxxxV(2,50).*x4_cu.*x5_cu.*x5_cu+6.*gxxxV(2,51).*x4_cu.*x5_cu.*x6_cu+3.*gxxxV(2,52).*x4_cu.*x6_cu.*x6_cu+gxxxV(2,53).*x5_cu.*x5_cu.*x5_cu+3.*gxxxV(2,54).*x5_cu.*x5_cu.*x6_cu+3.*gxxxV(2,55).*x5_cu.*x6_cu.*x6_cu+gxxxV(2,56).*x6_cu.*x6_cu.*x6_cu;
    evf_cu = evf_cu+gxxxV(3,1).*x1_cu.*x1_cu.*x1_cu+3.*gxxxV(3,2).*x1_cu.*x1_cu.*x2_cu+3.*gxxxV(3,3).*x1_cu.*x1_cu.*x3_cu+3.*gxxxV(3,4).*x1_cu.*x1_cu.*x4_cu+3.*gxxxV(3,5).*x1_cu.*x1_cu.*x5_cu+3.*gxxxV(3,6).*x1_cu.*x1_cu.*x6_cu+3.*gxxxV(3,7).*x1_cu.*x2_cu.*x2_cu+6.*gxxxV(3,8).*x1_cu.*x2_cu.*x3_cu+6.*gxxxV(3,9).*x1_cu.*x2_cu.*x4_cu+6.*gxxxV(3,10).*x1_cu.*x2_cu.*x5_cu+6.*gxxxV(3,11).*x1_cu.*x2_cu.*x6_cu+3.*gxxxV(3,12).*x1_cu.*x3_cu.*x3_cu+6.*gxxxV(3,13).*x1_cu.*x3_cu.*x4_cu+6.*gxxxV(3,14).*x1_cu.*x3_cu.*x5_cu+6.*gxxxV(3,15).*x1_cu.*x3_cu.*x6_cu+3.*gxxxV(3,16).*x1_cu.*x4_cu.*x4_cu+6.*gxxxV(3,17).*x1_cu.*x4_cu.*x5_cu+6.*gxxxV(3,18).*x1_cu.*x4_cu.*x6_cu+3.*gxxxV(3,19).*x1_cu.*x5_cu.*x5_cu+6.*gxxxV(3,20).*x1_cu.*x5_cu.*x6_cu+3.*gxxxV(3,21).*x1_cu.*x6_cu.*x6_cu+gxxxV(3,22).*x2_cu.*x2_cu.*x2_cu+3.*gxxxV(3,23).*x2_cu.*x2_cu.*x3_cu+3.*gxxxV(3,24).*x2_cu.*x2_cu.*x4_cu+3.*gxxxV(3,25).*x2_cu.*x2_cu.*x5_cu+3.*gxxxV(3,26).*x2_cu.*x2_cu.*x6_cu+3.*gxxxV(3,27).*x2_cu.*x3_cu.*x3_cu+6.*gxxxV(3,28).*x2_cu.*x3_cu.*x4_cu+6.*gxxxV(3,29).*x2_cu.*x3_cu.*x5_cu+6.*gxxxV(3,30).*x2_cu.*x3_cu.*x6_cu+3.*gxxxV(3,31).*x2_cu.*x4_cu.*x4_cu+6.*gxxxV(3,32).*x2_cu.*x4_cu.*x5_cu+6.*gxxxV(3,33).*x2_cu.*x4_cu.*x6_cu+3.*gxxxV(3,34).*x2_cu.*x5_cu.*x5_cu+6.*gxxxV(3,35).*x2_cu.*x5_cu.*x6_cu+3.*gxxxV(3,36).*x2_cu.*x6_cu.*x6_cu+gxxxV(3,37).*x3_cu.*x3_cu.*x3_cu+3.*gxxxV(3,38).*x3_cu.*x3_cu.*x4_cu+3.*gxxxV(3,39).*x3_cu.*x3_cu.*x5_cu+3.*gxxxV(3,40).*x3_cu.*x3_cu.*x6_cu+3.*gxxxV(3,41).*x3_cu.*x4_cu.*x4_cu+6.*gxxxV(3,42).*x3_cu.*x4_cu.*x5_cu+6.*gxxxV(3,43).*x3_cu.*x4_cu.*x6_cu+3.*gxxxV(3,44).*x3_cu.*x5_cu.*x5_cu+6.*gxxxV(3,45).*x3_cu.*x5_cu.*x6_cu+3.*gxxxV(3,46).*x3_cu.*x6_cu.*x6_cu+gxxxV(3,47).*x4_cu.*x4_cu.*x4_cu+3.*gxxxV(3,48).*x4_cu.*x4_cu.*x5_cu+3.*gxxxV(3,49).*x4_cu.*x4_cu.*x6_cu+3.*gxxxV(3,50).*x4_cu.*x5_cu.*x5_cu+6.*gxxxV(3,51).*x4_cu.*x5_cu.*x6_cu+3.*gxxxV(3,52).*x4_cu.*x6_cu.*x6_cu+gxxxV(3,53).*x5_cu.*x5_cu.*x5_cu+3.*gxxxV(3,54).*x5_cu.*x5_cu.*x6_cu+3.*gxxxV(3,55).*x5_cu.*x6_cu.*x6_cu+gxxxV(3,56).*x6_cu.*x6_cu.*x6_cu;
end
 
xVec_cup = zeros(numX,nx);
mx = setup.mx;
if mx > 0 
end 
if setup.myx > 0 
    xVec_cup(:,mx+1) = c_cu(:,1)-gSS(1,1);
end 
diaghx = diag(setup.hx);
xVec_cup(:,nx1+1:nx)  = x_cu(:,nx1+1:nx).*repmat(transpose(diaghx(nx1+1:nx)),numX,1); 
 
% The controls 
c_cu = repmat(c_cu,1,nNodes);
pai_cu = repmat(pai_cu,1,nNodes);
evf_cu = repmat(evf_cu,1,nNodes);
 
%Adding shocks to the states at t + 1 
diagEta = diag(setup.eta(nx1+1:nx,:)); 
x1_cup  = repmat(xVec_cup(:,1),1,nNodes);
x2_cup  = repmat(xVec_cup(:,2),1,nNodes) +2./(exp(-OMEGAz.*x_cu(:,2))+1).*diagEta(1,1).*repmat(epsNodes(1,:),numX,1);
x3_cup  = repmat(xVec_cup(:,3),1,nNodes) +2./(exp(-OMEGAd.*x_cu(:,3))+1).*diagEta(2,1).*repmat(epsNodes(2,:),numX,1);
x4_cup  = repmat(xVec_cup(:,4),1,nNodes) +2./(exp(-OMEGAn.*x_cu(:,4))+1).*diagEta(3,1).*repmat(epsNodes(3,:),numX,1);
x5_cup  = repmat(xVec_cup(:,5),1,nNodes) +2./(exp(-OMEGAp.*x_cu(:,5))+1).*diagEta(4,1).*repmat(epsNodes(4,:),numX,1);
x6_cup  = repmat(xVec_cup(:,6),1,nNodes) +2./(exp(-OMEGAa.*x_cu(:,6))+1).*diagEta(5,1).*repmat(epsNodes(5,:),numX,1);
 
% Scaling the states at time t+1 by the stdX
x1_cup  = x1_cup/scaleX_in_g(1,1);
x2_cup  = x2_cup/scaleX_in_g(2,1);
x3_cup  = x3_cup/scaleX_in_g(3,1);
x4_cup  = x4_cup/scaleX_in_g(4,1);
x5_cup  = x5_cup/scaleX_in_g(5,1);
x6_cup  = x6_cup/scaleX_in_g(6,1);
 
% Getting the controls at time t+1; each element is numX * nNodes
c_cup = g0(1,1)+gx(1,1).*x1_cup+gx(1,2).*x2_cup+gx(1,3).*x3_cup+gx(1,4).*x4_cup+gx(1,5).*x5_cup+gx(1,6).*x6_cup;
pai_cup = g0(2,1)+gx(2,1).*x1_cup+gx(2,2).*x2_cup+gx(2,3).*x3_cup+gx(2,4).*x4_cup+gx(2,5).*x5_cup+gx(2,6).*x6_cup;
evf_cup = g0(3,1)+gx(3,1).*x1_cup+gx(3,2).*x2_cup+gx(3,3).*x3_cup+gx(3,4).*x4_cup+gx(3,5).*x5_cup+gx(3,6).*x6_cup;
if appOrder > 1
    c_cup = c_cup+gxxV(1,1).*x1_cup.*x1_cup+2.*gxxV(1,2).*x1_cup.*x2_cup+2.*gxxV(1,3).*x1_cup.*x3_cup+2.*gxxV(1,4).*x1_cup.*x4_cup+2.*gxxV(1,5).*x1_cup.*x5_cup+2.*gxxV(1,6).*x1_cup.*x6_cup+gxxV(1,7).*x2_cup.*x2_cup+2.*gxxV(1,8).*x2_cup.*x3_cup+2.*gxxV(1,9).*x2_cup.*x4_cup+2.*gxxV(1,10).*x2_cup.*x5_cup+2.*gxxV(1,11).*x2_cup.*x6_cup+gxxV(1,12).*x3_cup.*x3_cup+2.*gxxV(1,13).*x3_cup.*x4_cup+2.*gxxV(1,14).*x3_cup.*x5_cup+2.*gxxV(1,15).*x3_cup.*x6_cup+gxxV(1,16).*x4_cup.*x4_cup+2.*gxxV(1,17).*x4_cup.*x5_cup+2.*gxxV(1,18).*x4_cup.*x6_cup+gxxV(1,19).*x5_cup.*x5_cup+2.*gxxV(1,20).*x5_cup.*x6_cup+gxxV(1,21).*x6_cup.*x6_cup;
    pai_cup = pai_cup+gxxV(2,1).*x1_cup.*x1_cup+2.*gxxV(2,2).*x1_cup.*x2_cup+2.*gxxV(2,3).*x1_cup.*x3_cup+2.*gxxV(2,4).*x1_cup.*x4_cup+2.*gxxV(2,5).*x1_cup.*x5_cup+2.*gxxV(2,6).*x1_cup.*x6_cup+gxxV(2,7).*x2_cup.*x2_cup+2.*gxxV(2,8).*x2_cup.*x3_cup+2.*gxxV(2,9).*x2_cup.*x4_cup+2.*gxxV(2,10).*x2_cup.*x5_cup+2.*gxxV(2,11).*x2_cup.*x6_cup+gxxV(2,12).*x3_cup.*x3_cup+2.*gxxV(2,13).*x3_cup.*x4_cup+2.*gxxV(2,14).*x3_cup.*x5_cup+2.*gxxV(2,15).*x3_cup.*x6_cup+gxxV(2,16).*x4_cup.*x4_cup+2.*gxxV(2,17).*x4_cup.*x5_cup+2.*gxxV(2,18).*x4_cup.*x6_cup+gxxV(2,19).*x5_cup.*x5_cup+2.*gxxV(2,20).*x5_cup.*x6_cup+gxxV(2,21).*x6_cup.*x6_cup;
    evf_cup = evf_cup+gxxV(3,1).*x1_cup.*x1_cup+2.*gxxV(3,2).*x1_cup.*x2_cup+2.*gxxV(3,3).*x1_cup.*x3_cup+2.*gxxV(3,4).*x1_cup.*x4_cup+2.*gxxV(3,5).*x1_cup.*x5_cup+2.*gxxV(3,6).*x1_cup.*x6_cup+gxxV(3,7).*x2_cup.*x2_cup+2.*gxxV(3,8).*x2_cup.*x3_cup+2.*gxxV(3,9).*x2_cup.*x4_cup+2.*gxxV(3,10).*x2_cup.*x5_cup+2.*gxxV(3,11).*x2_cup.*x6_cup+gxxV(3,12).*x3_cup.*x3_cup+2.*gxxV(3,13).*x3_cup.*x4_cup+2.*gxxV(3,14).*x3_cup.*x5_cup+2.*gxxV(3,15).*x3_cup.*x6_cup+gxxV(3,16).*x4_cup.*x4_cup+2.*gxxV(3,17).*x4_cup.*x5_cup+2.*gxxV(3,18).*x4_cup.*x6_cup+gxxV(3,19).*x5_cup.*x5_cup+2.*gxxV(3,20).*x5_cup.*x6_cup+gxxV(3,21).*x6_cup.*x6_cup;
end
if appOrder > 2
    c_cup = c_cup+gxxxV(1,1).*x1_cup.*x1_cup.*x1_cup+3.*gxxxV(1,2).*x1_cup.*x1_cup.*x2_cup+3.*gxxxV(1,3).*x1_cup.*x1_cup.*x3_cup+3.*gxxxV(1,4).*x1_cup.*x1_cup.*x4_cup+3.*gxxxV(1,5).*x1_cup.*x1_cup.*x5_cup+3.*gxxxV(1,6).*x1_cup.*x1_cup.*x6_cup+3.*gxxxV(1,7).*x1_cup.*x2_cup.*x2_cup+6.*gxxxV(1,8).*x1_cup.*x2_cup.*x3_cup+6.*gxxxV(1,9).*x1_cup.*x2_cup.*x4_cup+6.*gxxxV(1,10).*x1_cup.*x2_cup.*x5_cup+6.*gxxxV(1,11).*x1_cup.*x2_cup.*x6_cup+3.*gxxxV(1,12).*x1_cup.*x3_cup.*x3_cup+6.*gxxxV(1,13).*x1_cup.*x3_cup.*x4_cup+6.*gxxxV(1,14).*x1_cup.*x3_cup.*x5_cup+6.*gxxxV(1,15).*x1_cup.*x3_cup.*x6_cup+3.*gxxxV(1,16).*x1_cup.*x4_cup.*x4_cup+6.*gxxxV(1,17).*x1_cup.*x4_cup.*x5_cup+6.*gxxxV(1,18).*x1_cup.*x4_cup.*x6_cup+3.*gxxxV(1,19).*x1_cup.*x5_cup.*x5_cup+6.*gxxxV(1,20).*x1_cup.*x5_cup.*x6_cup+3.*gxxxV(1,21).*x1_cup.*x6_cup.*x6_cup+gxxxV(1,22).*x2_cup.*x2_cup.*x2_cup+3.*gxxxV(1,23).*x2_cup.*x2_cup.*x3_cup+3.*gxxxV(1,24).*x2_cup.*x2_cup.*x4_cup+3.*gxxxV(1,25).*x2_cup.*x2_cup.*x5_cup+3.*gxxxV(1,26).*x2_cup.*x2_cup.*x6_cup+3.*gxxxV(1,27).*x2_cup.*x3_cup.*x3_cup+6.*gxxxV(1,28).*x2_cup.*x3_cup.*x4_cup+6.*gxxxV(1,29).*x2_cup.*x3_cup.*x5_cup+6.*gxxxV(1,30).*x2_cup.*x3_cup.*x6_cup+3.*gxxxV(1,31).*x2_cup.*x4_cup.*x4_cup+6.*gxxxV(1,32).*x2_cup.*x4_cup.*x5_cup+6.*gxxxV(1,33).*x2_cup.*x4_cup.*x6_cup+3.*gxxxV(1,34).*x2_cup.*x5_cup.*x5_cup+6.*gxxxV(1,35).*x2_cup.*x5_cup.*x6_cup+3.*gxxxV(1,36).*x2_cup.*x6_cup.*x6_cup+gxxxV(1,37).*x3_cup.*x3_cup.*x3_cup+3.*gxxxV(1,38).*x3_cup.*x3_cup.*x4_cup+3.*gxxxV(1,39).*x3_cup.*x3_cup.*x5_cup+3.*gxxxV(1,40).*x3_cup.*x3_cup.*x6_cup+3.*gxxxV(1,41).*x3_cup.*x4_cup.*x4_cup+6.*gxxxV(1,42).*x3_cup.*x4_cup.*x5_cup+6.*gxxxV(1,43).*x3_cup.*x4_cup.*x6_cup+3.*gxxxV(1,44).*x3_cup.*x5_cup.*x5_cup+6.*gxxxV(1,45).*x3_cup.*x5_cup.*x6_cup+3.*gxxxV(1,46).*x3_cup.*x6_cup.*x6_cup+gxxxV(1,47).*x4_cup.*x4_cup.*x4_cup+3.*gxxxV(1,48).*x4_cup.*x4_cup.*x5_cup+3.*gxxxV(1,49).*x4_cup.*x4_cup.*x6_cup+3.*gxxxV(1,50).*x4_cup.*x5_cup.*x5_cup+6.*gxxxV(1,51).*x4_cup.*x5_cup.*x6_cup+3.*gxxxV(1,52).*x4_cup.*x6_cup.*x6_cup+gxxxV(1,53).*x5_cup.*x5_cup.*x5_cup+3.*gxxxV(1,54).*x5_cup.*x5_cup.*x6_cup+3.*gxxxV(1,55).*x5_cup.*x6_cup.*x6_cup+gxxxV(1,56).*x6_cup.*x6_cup.*x6_cup;
    pai_cup = pai_cup+gxxxV(2,1).*x1_cup.*x1_cup.*x1_cup+3.*gxxxV(2,2).*x1_cup.*x1_cup.*x2_cup+3.*gxxxV(2,3).*x1_cup.*x1_cup.*x3_cup+3.*gxxxV(2,4).*x1_cup.*x1_cup.*x4_cup+3.*gxxxV(2,5).*x1_cup.*x1_cup.*x5_cup+3.*gxxxV(2,6).*x1_cup.*x1_cup.*x6_cup+3.*gxxxV(2,7).*x1_cup.*x2_cup.*x2_cup+6.*gxxxV(2,8).*x1_cup.*x2_cup.*x3_cup+6.*gxxxV(2,9).*x1_cup.*x2_cup.*x4_cup+6.*gxxxV(2,10).*x1_cup.*x2_cup.*x5_cup+6.*gxxxV(2,11).*x1_cup.*x2_cup.*x6_cup+3.*gxxxV(2,12).*x1_cup.*x3_cup.*x3_cup+6.*gxxxV(2,13).*x1_cup.*x3_cup.*x4_cup+6.*gxxxV(2,14).*x1_cup.*x3_cup.*x5_cup+6.*gxxxV(2,15).*x1_cup.*x3_cup.*x6_cup+3.*gxxxV(2,16).*x1_cup.*x4_cup.*x4_cup+6.*gxxxV(2,17).*x1_cup.*x4_cup.*x5_cup+6.*gxxxV(2,18).*x1_cup.*x4_cup.*x6_cup+3.*gxxxV(2,19).*x1_cup.*x5_cup.*x5_cup+6.*gxxxV(2,20).*x1_cup.*x5_cup.*x6_cup+3.*gxxxV(2,21).*x1_cup.*x6_cup.*x6_cup+gxxxV(2,22).*x2_cup.*x2_cup.*x2_cup+3.*gxxxV(2,23).*x2_cup.*x2_cup.*x3_cup+3.*gxxxV(2,24).*x2_cup.*x2_cup.*x4_cup+3.*gxxxV(2,25).*x2_cup.*x2_cup.*x5_cup+3.*gxxxV(2,26).*x2_cup.*x2_cup.*x6_cup+3.*gxxxV(2,27).*x2_cup.*x3_cup.*x3_cup+6.*gxxxV(2,28).*x2_cup.*x3_cup.*x4_cup+6.*gxxxV(2,29).*x2_cup.*x3_cup.*x5_cup+6.*gxxxV(2,30).*x2_cup.*x3_cup.*x6_cup+3.*gxxxV(2,31).*x2_cup.*x4_cup.*x4_cup+6.*gxxxV(2,32).*x2_cup.*x4_cup.*x5_cup+6.*gxxxV(2,33).*x2_cup.*x4_cup.*x6_cup+3.*gxxxV(2,34).*x2_cup.*x5_cup.*x5_cup+6.*gxxxV(2,35).*x2_cup.*x5_cup.*x6_cup+3.*gxxxV(2,36).*x2_cup.*x6_cup.*x6_cup+gxxxV(2,37).*x3_cup.*x3_cup.*x3_cup+3.*gxxxV(2,38).*x3_cup.*x3_cup.*x4_cup+3.*gxxxV(2,39).*x3_cup.*x3_cup.*x5_cup+3.*gxxxV(2,40).*x3_cup.*x3_cup.*x6_cup+3.*gxxxV(2,41).*x3_cup.*x4_cup.*x4_cup+6.*gxxxV(2,42).*x3_cup.*x4_cup.*x5_cup+6.*gxxxV(2,43).*x3_cup.*x4_cup.*x6_cup+3.*gxxxV(2,44).*x3_cup.*x5_cup.*x5_cup+6.*gxxxV(2,45).*x3_cup.*x5_cup.*x6_cup+3.*gxxxV(2,46).*x3_cup.*x6_cup.*x6_cup+gxxxV(2,47).*x4_cup.*x4_cup.*x4_cup+3.*gxxxV(2,48).*x4_cup.*x4_cup.*x5_cup+3.*gxxxV(2,49).*x4_cup.*x4_cup.*x6_cup+3.*gxxxV(2,50).*x4_cup.*x5_cup.*x5_cup+6.*gxxxV(2,51).*x4_cup.*x5_cup.*x6_cup+3.*gxxxV(2,52).*x4_cup.*x6_cup.*x6_cup+gxxxV(2,53).*x5_cup.*x5_cup.*x5_cup+3.*gxxxV(2,54).*x5_cup.*x5_cup.*x6_cup+3.*gxxxV(2,55).*x5_cup.*x6_cup.*x6_cup+gxxxV(2,56).*x6_cup.*x6_cup.*x6_cup;
    evf_cup = evf_cup+gxxxV(3,1).*x1_cup.*x1_cup.*x1_cup+3.*gxxxV(3,2).*x1_cup.*x1_cup.*x2_cup+3.*gxxxV(3,3).*x1_cup.*x1_cup.*x3_cup+3.*gxxxV(3,4).*x1_cup.*x1_cup.*x4_cup+3.*gxxxV(3,5).*x1_cup.*x1_cup.*x5_cup+3.*gxxxV(3,6).*x1_cup.*x1_cup.*x6_cup+3.*gxxxV(3,7).*x1_cup.*x2_cup.*x2_cup+6.*gxxxV(3,8).*x1_cup.*x2_cup.*x3_cup+6.*gxxxV(3,9).*x1_cup.*x2_cup.*x4_cup+6.*gxxxV(3,10).*x1_cup.*x2_cup.*x5_cup+6.*gxxxV(3,11).*x1_cup.*x2_cup.*x6_cup+3.*gxxxV(3,12).*x1_cup.*x3_cup.*x3_cup+6.*gxxxV(3,13).*x1_cup.*x3_cup.*x4_cup+6.*gxxxV(3,14).*x1_cup.*x3_cup.*x5_cup+6.*gxxxV(3,15).*x1_cup.*x3_cup.*x6_cup+3.*gxxxV(3,16).*x1_cup.*x4_cup.*x4_cup+6.*gxxxV(3,17).*x1_cup.*x4_cup.*x5_cup+6.*gxxxV(3,18).*x1_cup.*x4_cup.*x6_cup+3.*gxxxV(3,19).*x1_cup.*x5_cup.*x5_cup+6.*gxxxV(3,20).*x1_cup.*x5_cup.*x6_cup+3.*gxxxV(3,21).*x1_cup.*x6_cup.*x6_cup+gxxxV(3,22).*x2_cup.*x2_cup.*x2_cup+3.*gxxxV(3,23).*x2_cup.*x2_cup.*x3_cup+3.*gxxxV(3,24).*x2_cup.*x2_cup.*x4_cup+3.*gxxxV(3,25).*x2_cup.*x2_cup.*x5_cup+3.*gxxxV(3,26).*x2_cup.*x2_cup.*x6_cup+3.*gxxxV(3,27).*x2_cup.*x3_cup.*x3_cup+6.*gxxxV(3,28).*x2_cup.*x3_cup.*x4_cup+6.*gxxxV(3,29).*x2_cup.*x3_cup.*x5_cup+6.*gxxxV(3,30).*x2_cup.*x3_cup.*x6_cup+3.*gxxxV(3,31).*x2_cup.*x4_cup.*x4_cup+6.*gxxxV(3,32).*x2_cup.*x4_cup.*x5_cup+6.*gxxxV(3,33).*x2_cup.*x4_cup.*x6_cup+3.*gxxxV(3,34).*x2_cup.*x5_cup.*x5_cup+6.*gxxxV(3,35).*x2_cup.*x5_cup.*x6_cup+3.*gxxxV(3,36).*x2_cup.*x6_cup.*x6_cup+gxxxV(3,37).*x3_cup.*x3_cup.*x3_cup+3.*gxxxV(3,38).*x3_cup.*x3_cup.*x4_cup+3.*gxxxV(3,39).*x3_cup.*x3_cup.*x5_cup+3.*gxxxV(3,40).*x3_cup.*x3_cup.*x6_cup+3.*gxxxV(3,41).*x3_cup.*x4_cup.*x4_cup+6.*gxxxV(3,42).*x3_cup.*x4_cup.*x5_cup+6.*gxxxV(3,43).*x3_cup.*x4_cup.*x6_cup+3.*gxxxV(3,44).*x3_cup.*x5_cup.*x5_cup+6.*gxxxV(3,45).*x3_cup.*x5_cup.*x6_cup+3.*gxxxV(3,46).*x3_cup.*x6_cup.*x6_cup+gxxxV(3,47).*x4_cup.*x4_cup.*x4_cup+3.*gxxxV(3,48).*x4_cup.*x4_cup.*x5_cup+3.*gxxxV(3,49).*x4_cup.*x4_cup.*x6_cup+3.*gxxxV(3,50).*x4_cup.*x5_cup.*x5_cup+6.*gxxxV(3,51).*x4_cup.*x5_cup.*x6_cup+3.*gxxxV(3,52).*x4_cup.*x6_cup.*x6_cup+gxxxV(3,53).*x5_cup.*x5_cup.*x5_cup+3.*gxxxV(3,54).*x5_cup.*x5_cup.*x6_cup+3.*gxxxV(3,55).*x5_cup.*x6_cup.*x6_cup+gxxxV(3,56).*x6_cup.*x6_cup.*x6_cup;
end
 
% The states at time t + 1 
c_ba1p = hSS(1,1) + x1_cup*scaleX_in_g(1,1);
muz_cup = hSS(2,1) + x2_cup*scaleX_in_g(2,1);
d_cup = hSS(3,1) + x3_cup*scaleX_in_g(3,1);
n_cup = hSS(4,1) + x4_cup*scaleX_in_g(4,1);
paistar_cup = hSS(5,1) + x5_cup*scaleX_in_g(5,1);
a_cup = hSS(6,1) + x6_cup*scaleX_in_g(6,1);
%% Saving the output 
% Getting the states at time t
out.c_ba1 = c_ba1;
out.muz_cu = muz_cu;
out.d_cu = d_cu;
out.n_cu = n_cu;
out.paistar_cu = paistar_cu;
out.a_cu = a_cu;
 
% Getting the states at time t+1
out.c_ba1p = c_ba1p;
out.muz_cup = muz_cup;
out.d_cup = d_cup;
out.n_cup = n_cup;
out.paistar_cup = paistar_cup;
out.a_cup = a_cup;
 
% Getting the controls at time t
out.c_cu = c_cu;
out.pai_cu = pai_cu;
out.evf_cu = evf_cu;
 
% Getting the controls at time t+1
out.c_cup = c_cup;
out.pai_cup = pai_cup;
out.evf_cup = evf_cup;
 
 
end
